package com.gitee.wsl.mathematics.complex.quaternion.ext


import com.gitee.wsl.mathematics.matrix.mat3.Matrix3
import com.gitee.wsl.mathematics.matrix.mat3.MutableMatrix3
import com.gitee.wsl.mathematics.number.compareTo
import com.gitee.wsl.mathematics.complex.quaternion.Quaternion

fun<T:Number> MutableMatrix3<T, *, *>.set(q: Quaternion<T, *>){
    val qxx = q.x * q.x
    val qyy = q.y * q.y
    val qzz = q.z * q.z
    val qxz = q.x * q.z
    val qxy = q.x * q.y
    val qyz = q.y * q.z
    val qwx = q.w * q.x
    val qwy = q.w * q.y
    val qwz = q.w * q.z

    this[0, 0] = 1f - 2f * (qyy + qzz)
    this[0, 1] = 2f * (qxy + qwz)
    this[0, 2] = 2f * (qxz - qwy)

    this[1, 0] = 2f * (qxy - qwz)
    this[1, 1] = 1f - 2f * (qxx + qzz)
    this[1, 2] = 2f * (qyz + qwx)

    this[2, 0] = 2f * (qxz + qwy)
    this[2, 1] = 2f * (qyz - qwx)
    this[2, 2] = 1f - 2f * (qxx + qyy)
}

fun<T:Number> Quaternion<T, *>.set(a: Matrix3<T, *, *>)=
    set(
        a.m00, a.m01, a.m02,
        a.m10, a.m11, a.m12,
        a.m20, a.m21, a.m22
    )

fun<T:Number> Quaternion<T, *>.set(
    m00: T, m01: T, m02: T,
    m10: T, m11: T, m12: T,
    m20: T, m21: T, m22: T
    ){

    val fourXSquaredMinus1 = m00 - m11 - m22
    val fourYSquaredMinus1 = m11 - m00 - m22
    val fourZSquaredMinus1 = m22 - m00 - m11
    val fourWSquaredMinus1 = m00 + m11 + m22

    var biggestIndex = 0
    var fourBiggestSquaredMinus1 = fourWSquaredMinus1
    if (fourXSquaredMinus1 > fourBiggestSquaredMinus1) {
        fourBiggestSquaredMinus1 = fourXSquaredMinus1
        biggestIndex = 1
    }
    if (fourYSquaredMinus1 > fourBiggestSquaredMinus1) {
        fourBiggestSquaredMinus1 = fourYSquaredMinus1
        biggestIndex = 2
    }
    if (fourZSquaredMinus1 > fourBiggestSquaredMinus1) {
        fourBiggestSquaredMinus1 = fourZSquaredMinus1
        biggestIndex = 3
    }

    val biggestVal = sqrt(fourBiggestSquaredMinus1 + 1f) * 0.5f
    val mult = 0.25f / biggestVal

    return when (biggestIndex) {
        0 -> set(       biggestVal    , (m12 - m21) * mult, (m20 - m02) * mult, (m01 - m10) * mult)
        1 -> set((m12 - m21) * mult,        biggestVal,     (m01 + m10) * mult, (m20 + m02) * mult)
        2 -> set((m20 - m02) * mult, (m01 + m10) * mult,        biggestVal,     (m12 + m21) * mult)
        3 -> set((m01 - m10) * mult, (m20 + m02) * mult, (m12 + m21) * mult,        biggestVal)
        // Silence a -Wswitch-default warning in GCC. Should never actually get here. Assert is just for sanity.
        else -> {
            //assert(false)
            set(1f, 0f, 0f, 0f)
        }
    }
}

